//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this sample source code is subject to the terms of the Microsoft
// license agreement under which you licensed this sample source code. If
// you did not accept the terms of the license agreement, you are not
// authorized to use this sample source code. For the terms of the license,
// please see the license agreement between you and Microsoft or, if applicable,
// see the LICENSE.RTF on your install media or the root of your tools installation.
// THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
//
//
// (C) Copyright 2006 Marvell International Ltd.
// All Rights Reserved
//

#include <windows.h>
#include <ceddk.h>

#include "Cs.h"
#include "Csmedia.h"

#include "CameraPDDProps.h"
#include "dstruct.h"
#include "dbgsettings.h"
#include <camera.h>
#include "CameraDriver.h"
#include "PinDriver.h"
#include "SensorFormats.h"

#include "monahans.h"
#include "ov7670.h"
#include "xllp_ov7670_hw.h"
#include "xllp_camera.h"
#include "xllp_ost.h"
#include "xllp_i2c.h"
#include "xllp_gpio_plat.h"
#include "micco.h"
#include "GPX_XLLP.h"
#include "xllp_pm_micco.h"
#include "xllp_camera_os_depend.h"

extern "C" unsigned int gpio_reg;
extern "C" unsigned int ost_reg;
extern "C" unsigned int i2c_reg;

BOOL pic_read_reg(HANDLE pic_drv, UCHAR addr, UCHAR* value);
BOOL pic_write_reg(HANDLE pic_drv, UCHAR addr, UCHAR value);

static UINT get_gain();
static void set_gain(UINT gain);
static UINT get_exposure();
static void set_exposure(UINT exposure);
static void set_exposure_gain(UINT preview_exposure, UINT preview_gain);
static void get_ae(UCHAR* pre_gain, UCHAR* r_gain, UCHAR* b_gain);
static void set_ae(UCHAR pre_gain, UCHAR r_gain, UCHAR b_gain);

static MAKE_STREAM_MODE_YV12(DCAM_StreamMode_CIF_YV12, 352, -288, 12, 15);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_QCIF_YV12, 176, -144, 12, 15);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_QVGA_YV12, 320, -240, 12, 30);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_VGA_YV12, 640, -480, 12, 30);

static qci_interface_t ov7670_interface =
{
    XLLP_CI_MODE_MP,
    XLLP_CI_DATA_WIDTH8,
    2600,
    XLLP_FALSE, 
    XLLP_FALSE, 
    XLLP_FALSE
};

static camera_cfg_t preview_cfg = 
{
    &DCAM_StreamMode_CIF_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,     /* sensor format */
    CIF,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,
        352, 288
    },
    0,
};

static camera_cfg_t qcif_preview_cfg = 
{
    &DCAM_StreamMode_QCIF_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,     /* sensor format */
    QCIF,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,
        176, 144
    },
    0,
};

static camera_cfg_t still_cfg = 
{
    &DCAM_StreamMode_VGA_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,     /* sensor format */
    VGA,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,
        640, 480
    },
    0,
};

MAKE_STREAM_MODE_RGB565(DCAM_StreamMode_0, 160, 120, 16, 30);
MAKE_STREAM_MODE_RGB565(DCAM_StreamMode_1, 176, 144, 16, 30);
MAKE_STREAM_MODE_RGB565(DCAM_StreamMode_2, 320, 240, 16, 15);
MAKE_STREAM_MODE_RGB565(DCAM_StreamMode_3, 320, 240, 16, 30);
MAKE_STREAM_MODE_RGB565(DCAM_StreamMode_4, 640, 480, 16, 15);

MAKE_STREAM_MODE_YV12(DCAM_StreamMode_11, 400, -294, 12, 30);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_12, 800, -600, 12, 30);
//MAKE_STREAM_MODE_YV12(DCAM_StreamMode_UXGA_YV12, 1600, -1200, 12, 15);

#define SENSOR_INIT_WAIT  150
void map_registers();
static void dump_ov7670();

Ov7670::Ov7670()
{
    map_registers();
    qci_interface = &ov7670_interface;
    timing.BFW = 0x01;
    timing.BLW = 0x00;  
	DCAM_StreamMode_QCIF_YV12.ConfigCaps.MinFrameInterval = 632911;
	DCAM_StreamMode_QCIF_YV12.VideoInfoHeader.AvgTimePerFrame = 632911;
}

void set_voltage(UCHAR camera_ana, UCHAR camera_io, UCHAR camera_core, BOOL is_on)
{
    HANDLE pic_drv;
    
    // camera_ana.
    const UCHAR V_CAM_ANA_REG = 0x13;
    const UCHAR V_CAM_ANA_MASK = 0xE0;
    
    // camera_io.
    const UCHAR V_CI_REG = 0x17;
    const UCHAR V_CI_MASK = 0xF0;
    
    camera_ana <<= 5;
    camera_io  <<= 4;    

    pic_drv = CreateFile(PIC_FILE_NAME, 0, 0, NULL, 0, 0, NULL);

    if (pic_drv == INVALID_HANDLE_VALUE) {
        NKDbgPrintfW(L"camera: open pic file failed!\r\n");
    } else {
        UCHAR reg;
        
        // Set up V_CAM_ANA.
        pic_read_reg(pic_drv, V_CAM_ANA_REG, &reg);        
        if ((reg & V_CAM_ANA_MASK) != camera_ana) {
            pic_write_reg(pic_drv, V_CAM_ANA_REG, (reg & ~V_CAM_ANA_MASK) | camera_ana);
        }
        
        // Set up V_CI.
        pic_read_reg(pic_drv, V_CI_REG, &reg);        
        if ((reg &  V_CI_MASK) != camera_io) {
            pic_write_reg(pic_drv, V_CI_REG, (reg &  ~V_CI_MASK) | camera_io);
        }

#ifdef SETUP_IO_CORE_VOLTAGE  

        pic_read_reg(pic_drv, 0x13, &reg);
        pic_write_reg(pic_drv, 0x13, (reg & 0xf0) | camera_io);

        pic_read_reg(pic_drv, 0x92, &reg);
        if ((reg & 0xf) != camera_core)
            pic_write_reg(pic_drv, 0x92, (reg & 0xf0) | camera_core);
#endif       

        CloseHandle(pic_drv);
    }
}

bool Ov7670::detect()
{
    XLLP_UINT8_T pid = 0, rev = 0;
    OV7670VersionRevision(&pid, &rev);    
    return pid == PID_OV76XX && rev == PID_7670;
}

unsigned int Ov7670::get_formats(ULONG type, PCS_DATARANGE_VIDEO** formats)
{
    // Video Format initialization
    unsigned int nformats = 1;

    *formats = new PCS_DATARANGE_VIDEO[nformats];

    if( NULL == *formats )
    {
        return 0;
    }

    switch (type)
    {
    case CAPTURE:
    case PREVIEW:
        (*formats)[0] = &DCAM_StreamMode_CIF_YV12;
        break;
    case STILL:
        (*formats)[0] = &DCAM_StreamMode_VGA_YV12;
    default:
        break;
    }
    return nformats;
}

void Ov7670::start_capture()
{
    // Specified the exposure and gain value.
    USHORT exposure = 400;
    USHORT gain = 200;

    if (frame_size == VGA)
    {
        OV7670AutoFunctionOff();
        set_exposure_gain(exposure, gain);
        set_ae(pre_gain, r_gain, b_gain);        
    } else
        OV7670AutoFunctionOn();    
}

void Ov7670::stop_capture()
{
    if (frame_size != VGA)
    {
        //NKDbgPrintfW(L"Ov7670::Get AE/AGC\r\n");
        gain = get_gain();
        exposure = get_exposure();
        get_ae(&pre_gain, &r_gain, &b_gain);
        //NKDbgPrintfW(L"Ov7670::Get AE/AGC, gain=%d, exposure=%d",gain,exposure);
    }
    
    // put sensor output in tri-state when power down
    UCHAR com3;
    OV7670ReadSensorReg(OV7670_COM3, &com3);
    com3 &= ~0x18;
    OV7670WriteSensorReg(OV7670_COM3, &com3);
}

void Ov7670::set_power_mode(bool is_on)
{
    set_voltage(0x0, 0x0, 0x0, is_on);

    XLLP_UINT8_T buffer[1];
    int status;
    const UCHAR GPX_ADDR = 0x50;
    const UCHAR GPX_CAMERA_POWER_DOWN_MASK = 0x20;
    
    //  Set up GPX power_down pins.
    status = OS_I2CMasterReadData(GPX_ADDR, buffer, 1);
    if (XLLP_STATUS_SUCCESS != status) {
        RETAILMSG(1, (TEXT("Failed to read from GPX address.\r\n")));
    } 
    
    if (is_on) {
        // Set CAMERA_POWER_DOWN to zero.
        buffer[0] &= ~GPX_CAMERA_POWER_DOWN_MASK;
        status = OS_I2CMasterWriteData(GPX_ADDR, buffer, 1);
        if (XLLP_STATUS_SUCCESS != status) {
            RETAILMSG(1, (TEXT("Failed to write GPX address.\r\n")));
        }        
    } else {
        // Set CAMERA_POWER_DOWN to 1.
        buffer[0] |= GPX_CAMERA_POWER_DOWN_MASK;
        status = OS_I2CMasterWriteData(GPX_ADDR, buffer, 1);
        if (XLLP_STATUS_SUCCESS != status) {
            RETAILMSG(1, (TEXT("Failed to write GPX address.\r\n")));
        }        
    }   

    if (is_on)
        Sleep(200);
}

camera_cfg_t* Ov7670::get_camera_cfg(PCS_VIDEOINFOHEADER pCsVideoInfoHdr, 
                                   ULONG mode)
{
    UINT width        = abs(pCsVideoInfoHdr->bmiHeader.biWidth);
    UINT height       = abs(pCsVideoInfoHdr->bmiHeader.biHeight);

    if (width == 176 && height == 144)
        return &qcif_preview_cfg;
    else if (width == 352 && height == 288)
        return &preview_cfg;
    else
        return &still_cfg;
}

extern void fill_buffer_yv12(PUCHAR* dest_plane, frame_t* frame);

void Ov7670::handle_frame_interrupt(PUCHAR buf,
                                    CS_BITMAPINFOHEADER* info_header,
                                    frame_t* qci_frame)
{
    //NKDbgPrintfW(L"Ov7670::handle_frame_interrupt, width = %d, height = %d\r\n", info_header->biWidth, info_header->biHeight);
    //NKDbgPrintfW(L"buf = 0x%08X, qci_frame = 0x%08X, width = %d, height = %d\r\n", buf, qci_frame, qci_frame->list->format.width, qci_frame->list->format.height);
    UINT width = abs(info_header->biWidth);
    UINT height = abs(info_header->biHeight);
    UINT y_size = width * height;   

    PUCHAR dest[3];

    dest[0] = buf;
    dest[1] = dest[0] + y_size;
    dest[2] = dest[1]  + (y_size >> 2);

    fill_buffer_yv12(dest, qci_frame);
}

void Ov7670::set_frame_format(int format, FrameSize size)
{
    int status;
    XLLP_UINT32_T ovSizeFormat, ovFormat;

    frame_size = size;
    // Set the current mode
    switch(format) {
        case XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR:
        case XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PACKED:
            ovFormat = OV_FORMAT_YUV_422;
            break;
        case XLLP_CAMERA_IMAGE_FORMAT_RGB565:
            ovFormat = OV_FORMAT_RGB_565;
            break;
        case XLLP_CAMERA_IMAGE_FORMAT_RAW8:
            ovFormat = OV_FORMAT_RAW8;
            break;
        default:
            ovFormat = OV_FORMAT_YUV_422;
            break;
    }

    switch(size)
    {
    case QCIF:
    	ovSizeFormat = OV_SIZE_QCIF;
        break;
    case CIF:
    	ovSizeFormat = OV_SIZE_CIF;
        break;
    case QQVGA:
        ovSizeFormat = OV_SIZE_QQVGA;
        break;
    case QVGA:
        ovSizeFormat = OV_SIZE_QVGA;
        break;
    case VGA:
    default:
        ovSizeFormat = OV_SIZE_VGA;
        break;
    }
 
    status = OV7670SetFormat(ovSizeFormat, ovFormat, 0);
}

void map_registers()
{
    PHYSICAL_ADDRESS phy_addr;

    if (!ost_reg)
    {
        phy_addr.QuadPart = MONAHANS_BASE_REG_PA_OST;
        ost_reg = (unsigned int)MmMapIoSpace(phy_addr, sizeof(XLLP_OST_T), FALSE);
    }

    if (!gpio_reg)
    {
        phy_addr.QuadPart = MONAHANS_BASE_REG_PA_GPIO;
        gpio_reg = (unsigned int)MmMapIoSpace(phy_addr, sizeof(XLLP_GPIO_T), FALSE);
    }

    if (!i2c_reg)
    {
        phy_addr.QuadPart = MONAHANS_BASE_REG_PA_I2C;
        i2c_reg = (unsigned int)MmMapIoSpace(phy_addr, sizeof(XLLP_I2C_T), FALSE);
    }    
}

static void dump_ov7670()
{
    unsigned char buf[0xC9];

    OV7670ReadAllRegs(buf, 0xC9);

    for (int i = 0; i < 0xc9; i++) {
        NKDbgPrintfW(L"reg 0x%02X = 0x%02X, ", i, buf[i]);
        i++;
        NKDbgPrintfW(L"reg 0x%02X = 0x%02X\r\n", i, buf[i]);
    }
}

BOOL pic_read_reg(HANDLE pic_drv, UCHAR addr, UCHAR* value)
{
    if(!DeviceIoControl(pic_drv, PIC_IO_GET_REG, &addr, sizeof(addr), 
                        value, sizeof(*value), NULL, NULL))
    {
        DEBUGMSG(ZONE_ERROR,(_T("Error PIC_IO_GET_REG Last Error %d.\r\n"),GetLastError() ));
        return FALSE;
    }
    return TRUE;
}

BOOL pic_write_reg(HANDLE pic_drv, UCHAR addr, UCHAR value)
{
    UCHAR input[2];
    
    input[0] = addr;
    input[1] = value;

    if(!DeviceIoControl(pic_drv, PIC_IO_SET_REG, input, sizeof(input), 
                        NULL,NULL,NULL,NULL))
    {
        DEBUGMSG(ZONE_ERROR,(_T("Error PIC_IO_SET_REG Last Error %d.\r\n"),GetLastError() ));
        return FALSE;
    }

    return TRUE;
}

static UINT get_gain()
{    
    UCHAR gain_reg;
    int gain;

    OV7670ReadSensorReg(OV7670_GAIN, &gain_reg);
    
    gain = (gain_reg & 0x0f) + 16;

    if (gain_reg & 0x10)
        gain = gain << 1;

    if (gain_reg & 0x20)
        gain = gain << 1;

    if (gain_reg & 0x40)
        gain = gain << 1;

    if (gain_reg & 0x80)
        gain = gain << 1;

    return gain;
}

static UINT get_exposure()
{
    UCHAR aechh, aech, com1;

    OV7670ReadSensorReg(OV7670_AECHH, &aechh);
    OV7670ReadSensorReg(OV7670_AECH, &aech);
    OV7670ReadSensorReg(OV7670_COM1, &com1);
    return ((aechh & 0x3f) << 10) + (aech << 2) + (com1 & 0x03);
}

static void set_exposure_gain(UINT preview_exposure, UINT preview_gain)
{
    // the following data are hardcode for Littleton
    UINT capture_maxline = 498;
    UINT preview_maxline = 326;
    UINT capture_f_rate = 10;
    UINT preview_f_rate = 16;
    BOOL is_50Hz = TRUE;        // if your power frequency is 60Hz, set to FALSE
    UINT lines_10ms = 0x4c;
    UINT exposure;
    UINT gain;
    UINT exposure_gain;

//Capture_Exposure = Preview_Exposure * Capture_Framerate * Capture_MaxLine / (Preview_Framerate * Preview_MaxLine);

    exposure = preview_exposure * capture_f_rate * capture_maxline 
                / (preview_f_rate * preview_maxline);

    exposure_gain = exposure * preview_gain;

    if (is_50Hz)
        lines_10ms = capture_f_rate * capture_maxline / 100;    
    else 
	lines_10ms = capture_f_rate * capture_maxline / 120; 

    if (exposure_gain < capture_maxline * 16)
    {
        exposure = exposure_gain / 16;

        if (exposure > lines_10ms) 
        {
            exposure /= lines_10ms;
            exposure *= lines_10ms;  //Capture_Exposure = n*Lines_10ms, Banding removed
        }
    }
    else
    {
        exposure = capture_maxline * lines_10ms / lines_10ms;
    }

    gain = (exposure_gain * 100 / exposure + 50) / 100;
    
    set_exposure(exposure);
    set_gain(gain);
}

static void set_exposure(UINT exposure)
{
    UCHAR com1;
    UCHAR reg;

    OV7670ReadSensorReg(OV7670_COM1, &com1);
    com1 = (com1 & 0xfc) | (exposure & 0x03);
    OV7670WriteSensorReg(OV7670_COM1, &com1);

    reg = (UCHAR)((exposure >> 2) & 0xff);
    OV7670WriteSensorReg(OV7670_AECH, &reg);

    reg = (UCHAR)((exposure >> 10) & 0xff);
    OV7670WriteSensorReg(OV7670_AECHH, &reg);
}

static void set_gain(UINT gain)
{
    UCHAR reg = 0;

    if (gain > 31)
    {
        reg |= 0x10;
        gain = gain >> 1;
    }	
    
    if (gain > 31)
    {
        reg |= 0x20;
        gain = gain >> 1;
    }	

    if (gain > 31)
    {
        reg |= 0x40;
        gain = gain >> 1;
    }	

    if (gain > 31)
    {
        reg |= 0x80;
        gain = gain >> 1;
    }	

    if (gain > 16)
        reg |= (gain -16) & 0x0f;

    OV7670WriteSensorReg(OV7670_AECHH, &reg);
}	

static void get_ae(UCHAR* pre_gain, UCHAR* r_gain, UCHAR* b_gain)
{
    OV7670ReadSensorReg(OV7670_GFIX, pre_gain);
    OV7670ReadSensorReg(OV7670_RED, r_gain);
    OV7670ReadSensorReg(OV7670_BLUE, b_gain);
}

static void set_ae(UCHAR pre_gain, UCHAR r_gain, UCHAR b_gain)
{
    OV7670WriteSensorReg(OV7670_GFIX, &pre_gain);
    OV7670WriteSensorReg(OV7670_RED, &r_gain);
    OV7670WriteSensorReg(OV7670_BLUE, &b_gain);
}
